#include "tcpcommunicate.h"
#include "qjsonarray.h"
#include <QDebug>
#include <QTimer>
#include <QJsonDocument>


//json心跳
const QByteArray CreateHeartBeat(QString sValue)
{
    //关节的全部映射
    QMap<QString, QVariant> mapHeartBeat = QMap<QString, QVariant>();
    //json头映射
    QMap<QString, QVariant> heartBeatObjMap; // 假设您已经有一个QMap<QString, int>对象
    heartBeatObjMap.insert("data_type",QString("heart_beat_%1").arg(sValue));
    heartBeatObjMap.insert("value",true);
    mapHeartBeat.insert("data_info",heartBeatObjMap);
    //map->json
    QJsonDocument doc = QJsonDocument::fromVariant(QVariant(mapHeartBeat));
    return doc.toJson(QJsonDocument::Compact);
}
//心跳常量
const QByteArray JSON_ROBOTHEARTBEAT=CreateHeartBeat("robot");
const QByteArray JSON_PDAHEARTBEAT=CreateHeartBeat("pda");

MyTcpClient::MyTcpClient(int iClientPort,QObject *parent)
    : QObject{parent}
{
    m_pTcpClient = new QTcpSocket(this);
    m_pTcpClient->connectToHost(QHostAddress::Any, iClientPort);
    connect(m_pTcpClient, &QTcpSocket::readyRead, this, [=]{
        qDebug()<<"got"<<m_pTcpClient->readAll();
    });

}
void MyTcpClient::slot_writeData(QByteArray  &ba)
{

}

MyTcpServer::MyTcpServer(int iServerPort,QObject *parent)
    : QObject{parent}
{
    //给服务器指针实例化对象
    m_pTcpServer = new QTcpServer(this);
    //将服务器设置为被动监听状态
    //bool QTcpServer::listen(const QHostAddress &address = QHostAddress::Any, quint16 port = 0)
    //参数1：要监听的主机地址，如果是any，表示监听所有主机地址，也可以给特定的主机地址进行监听
    //参数2：通过指定的端口号进行访问服务器，如果是0，表示由服务器自动分配。如果非0，则表示指定端口号
    //返回值：成功返回真，失败返回假
    if(!m_pTcpServer->listen(QHostAddress::Any,iServerPort))
    {
        qDebug("server failed");
    }else{
        qDebug("server success");
    }
    connect(m_pTcpServer, &QTcpServer::newConnection, this, &MyTcpServer::slot_onNewConnection);
    //启动定时器
    QTimer *pTimerHeartBeat=new QTimer(this);
    //连接定时器
    connect(pTimerHeartBeat,&QTimer::timeout,this,[=]{
        for(int i=0; i<pClientList.count(); i++)
        {
            pClientList[i]->write(JSON_ROBOTHEARTBEAT);
        }
    });
    pTimerHeartBeat->start(1000);

}

void MyTcpServer::slot_onNewConnection() {
    qDebug() <<"有客户端申请连接";
    //获取最新连接的客户端套接字
    QTcpSocket *pClientSocket = m_pTcpServer->nextPendingConnection();
    // 将客户端套接字保存到列表或进行其他处理
    //将获取的套接字存放到客户端容器中
    pClientList.push_back(pClientSocket);

    //此时，客户端就和服务器建立起来联系了
    //如果客户端有数据向服务器发送过来，那么该套接字就会自动发送一个readyread信号
    //我们可以将该信号连接到自定义的槽函数中处理相关逻辑
    connect(pClientSocket, &QTcpSocket::readyRead, this, [=]{
        //删除客户端链表中的无效客户端套接字
        for(int i=0; i<pClientList.count(); i++)
        {
            //判断套接字的状态
            //函数原型     SocketState state() const;
            //功能：返回客户端状态
            //返回值：客户端状态，如果是0，表示无连接
            if(pClientList[i]->state() == 0)
            {
                pClientList.removeAt(i);     //将下标为i的客户端移除
            }
        }

        //遍历所有客户端，查看是哪个客户端发来数据
        for(int i=0; i<pClientList.count(); i++)
        {
            //函数原型：qint64 bytesAvailable() const override;
            //功能：返回当前客户端套接字中的可读数据字节个数
            //返回值：当前客户端待读的字节数，如果该数据0，表示无待读数据
            if(pClientList[i]->bytesAvailable() != 0)
            {
                //读取当前客户端的相关数据
                //函数原型：QByteArray readAll();
                //功能：读取当前套接字中的所有数据，并返回一个字节数组
                //返回值：数据的字节数组
                QByteArray msg = pClientList[i]->readAll();

                //将数据战术到ui界面上
                // qDebug()<<"got one"<<msg;
                this->_handle(msg);

                //将接收到的该消息，发送给所有客户端
                // for(int j=0; j<pClientList.count(); j++)
                // {
                //     // pClientList[j]->write(msg);
                // }

            }
        }
    });

}
void MyTcpServer::_handle(const QByteArray &src)
{
    qDebug()<<"in tcp handle";
    if(src==JSON_PDAHEARTBEAT){
        qDebug()<<"got heartbeat"<<src;
    }else{
        //处理收到的json数据包 测试了心跳 ok
        QJsonParseError jsonError;
        QJsonDocument doucment = QJsonDocument::fromJson(src, &jsonError);  // 转化为 JSON 文档
        if (!doucment.isNull() && (jsonError.error == QJsonParseError::NoError)) {  // 解析未发生错误
            if (doucment.isObject()) { // JSON 文档为对象
                QJsonObject object = doucment.object();  // 转化为对象
                if (object.contains("data_info")) {  // 包含指定的 key
                    QJsonValue value_datainfo = object.value("data_info");  // 获取指定 key 对应的 value
                    if (value_datainfo.isObject()) {  // 判断 value 是否为对象
                        QJsonObject object_datainfo = value_datainfo.toObject();  // 转化为对象
                        if (object_datainfo.contains("data_type")){
                            QJsonValue value_datainfo_datatype = object_datainfo.value("data_type");  // 获取指定 key 对应的 value
                            if(value_datainfo_datatype.isString()){
                                QString sData_type_info = value_datainfo_datatype.toString();  // 将 value 转化为字符串

                                qDebug()<<"strName=data_type"<<sData_type_info;
                                if(sData_type_info=="command_emergency"){//一级指令——暂停/启动/停止（TCP）
                                    QString sData_value_command_emergency=object_datainfo.value("value").toString();
                                    this->_handleEmergency(sData_value_command_emergency);
                                }
                                else if(sData_type_info=="request_control"){//请求控制机器人
                                    bool bData_value_request_control=object_datainfo.value("value").toBool();
                                    this->_handleRequestControl(bData_value_request_control);
                                }else if(sData_type_info=="request_power"){//查询机器人开机状态
                                    bool bData_value_request_power=object_datainfo.value("value").toBool();
                                    this->_handleRequestPower(bData_value_request_power);
                                }else if(sData_type_info=="command_poweroff"){//关机指令
                                    bool bData_value_command_poweroff=object_datainfo.value("value").toBool();
                                    this->_handleCommandPoweroff(bData_value_command_poweroff);
                                }else if(sData_type_info=="command_robotstate"){//控制模式更改
                                    QString sData_value_command_robotstate=object_datainfo.value("value").toString();
                                    this->_handleCommandChangeRobotState(sData_value_command_robotstate);
                                }else if(sData_type_info=="request_gait"){//请求当前步态
                                    bool bData_value_request_gait=object_datainfo.value("value").toBool();
                                    this->_handleGait(bData_value_request_gait);
                                }else if(sData_type_info=="command_gait"){//发送更改步态指令
                                    QString sData_value_command_gait=object_datainfo.value("value").toString();
                                    this->_handleGaitCommand(sData_value_command_gait);
                                }else if(sData_type_info=="command_calibration"){//标定指令
                                    QString sData_value_command_calibration=object_datainfo.value("value").toString();
                                    this->_handleCalibration(sData_value_command_calibration);
                                }else if(sData_type_info=="command_robotstate_auto"){//自动模式下发送机器人位置控制指令
                                    QJsonValue value_datainfo = object.value("robot_move");
                                    if(value_datainfo.isObject())
                                        this->_handleCommandRootStateAuto(value_datainfo.toObject());
                                }else if(sData_type_info=="command_robotstate_manual"){//手动模式下更改机器人关节或者末端指令
                                    QJsonValue value_body_joints_info = object.value("body_joints");
                                    if(value_body_joints_info.isObject())
                                        this->_handleCommandRootStateMannual(value_body_joints_info.toObject(),MANNUAL_BODY_JOINTS);
                                    QJsonValue value_end_joints_info = object.value("endeffctor_joints");
                                    if(value_end_joints_info.isObject())
                                        this->_handleCommandRootStateMannual(value_end_joints_info.toObject(),MANNUAL_ENDEFFCTOR_JOINTS);
                                }
                            }

                        }
                    }

                }
            }
        }
    }

}
void MyTcpServer::_handleRequestControl(const bool bCommand)
{
    qDebug()<<"in tcp _handleControlRequest"<<bCommand;

}
void MyTcpServer::_handleRequestPower(const bool bCommand)
{
    qDebug()<<"in tcp _handleRequestPower"<<bCommand;
}
void MyTcpServer::_handleCommandPoweroff(const bool bCommand)
{
    qDebug()<<"in _handleCommandPoweroff"<<bCommand;
}

void MyTcpServer::_handleCommandChangeRobotState(const QString &sValue)
{
    qDebug()<<"in tcp _handleCommandChangeRobotState"<<sValue;
    QStringList stateWords;
    stateWords << "manual" << "auto" << "demonstrate"<< "teleoperation";
    switch (stateWords.indexOf(sValue)) {
    case 0:
        qDebug()<<"manual";
        break;
    case 1:
        qDebug()<<"auto";
        break;
    case 2:
        qDebug()<<"demonstrate";
        break;
    case 3:
        qDebug()<<"teleoperation";
        break;
    default:
        qDebug()<<"others";
        break;
    }
}

void MyTcpServer::_handleGait(const bool bCommand)
{
    qDebug()<<"in tcp _handleGait"<<bCommand;
}
void MyTcpServer::_handleGaitCommand(const QString &sValue)
{
    qDebug()<<"in tcp _handleGaitCommand"<<sValue;
    QStringList stateWords;
    stateWords << "manual" << "auto" << "demonstrate"<< "teleoperation";
}
void MyTcpServer::_handleCalibration(const QString &sValue)
{
    qDebug()<<"in _handleCalibration"<<sValue;
}

void MyTcpServer::_handleEndeffctorInfo()
{
    qDebug()<<"in tcp _handleEndeffctorInfo";
}
void MyTcpServer::_handleEmergency(const QString &sValue)
{
    qDebug()<<"in tcp _handleEmergency"<<sValue;
    QStringList stopWords;
    stopWords << "stop" << "run" << "pause";
    switch (stopWords.indexOf(sValue)) {
    case 0:
        qDebug()<<"stop";
        break;
    case 1:
        qDebug()<<"run";
        break;
    case 2:
        qDebug()<<"pause";
        break;

    default:
        qDebug()<<"others";
        break;
    }

}
void MyTcpServer::_handleCommandRootStateMannual(const QJsonObject &obj,const MannualStateEnum &state)
{
    qDebug()<<"in tcp _handleCommandRootStateMannual"<<state;
    //遍历输出
    QStringList keyList = obj.keys();
    for(QString key : keyList){
        if(obj[key].isString()){
            qDebug() << key << obj[key].toString();
        }else if(obj[key].isArray()){//获取手动控制模式下下发的关节位姿电流电压等的设置
            QJsonArray array=obj[key].toArray();
            for(int i=0;i<array.count();++i){
                qDebug()<<key<<QString("array%1").arg(i)<<array[i].toDouble();
            }
        }else if(obj[key].isDouble()){
            qDebug() << key << obj[key].toDouble();
        }
    }
}
/**
     * @introduce   传入的是robot_move
     * @author      毛辰飞
     * @data        2024/11/25
**/
void MyTcpServer::_handleCommandRootStateAuto(const QJsonObject &obj)
{
    qDebug()<<"in tcp _handleCommandRootStateAuto"<<obj;
    //遍历输出
    QStringList keyList = obj.keys();
    for(QString key : keyList){
        if(key=="specificmove"){//自动模式下发送固定动作指令
            qDebug() << key << obj[key].toString();
        }else{ //自动模式下发送机器人位置控制
            if(obj[key].isString())
                qDebug() << key << obj[key].toString();
            else if(obj[key].isDouble())
                qDebug() << key << obj[key].toDouble();
        }
    }
}

